/*
 *  sndrms.c
 *
 *  Author: Thomas Chang
 *  Date:  2005-08-20
 *
 *  Calculate the RMS (root-mean-squre) of sound data. Update silence threshold dynamically.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <fcntl.h>

#include "sndrms.h"
//#define __DEBUG

#include "Trace.h"

#define MAX_HWORD 	32767

/*
 * per input sound device thread - read sound sample data from device and
 * output to each client
 */
/* return value is 100 x RMS (data) */
static long calculate_rms( short *data, int nsample )
{
	long long rms_sum = 0;
	double	  d_rms_sum;
	int	i;

	for(i=0; i<nsample; i++)
		rms_sum += data[i] * data[i];
	rms_sum *= 10000;
	rms_sum /= nsample;
	d_rms_sum = rms_sum;
	rms_sum = sqrt( d_rms_sum );
	return (long) rms_sum;
}

static long calculate_stddev( long *data, int nsample, long *data_avg )
{
	long long	sum = 0;
	long		avg;
	long		stddev = 0;
	double		dstddev;
	long		delta;
	int		i;

	for(i=0; i<nsample; i++)
		sum += data[i];
	avg = sum / nsample;
	for (i=0; i<nsample; i++)
	{
		delta = data[i] - avg;
		stddev += delta * delta;
	}
	dstddev = stddev;
	dstddev /= (nsample - 1);
	stddev = (long)sqrt(dstddev);
	*data_avg = avg;
	return stddev;
}

static void RMSW_init( RMSWindow *rmsw, int threshold )
{
	memset( rmsw, 0, sizeof(RMSWindow) );
	rmsw->silence_threshold = threshold;
	rmsw->thresholdx2 	= threshold;
	rmsw->window_start	= rmsw->silence_window;
	rmsw->window_end	= rmsw->silence_window + WINDOW_SIZE;
	rmsw->window_ptr	= rmsw->silence_window;
	rmsw->silence_count	= SILENCE_LIMIT;
}

RMSWindow *RMSW_new( int threshold_init )
{
	RMSWindow	*rmsw = malloc( sizeof(RMSWindow) );
	if ( rmsw )
		RMSW_init( rmsw, threshold_init );
	return rmsw;
}

int RMSW_isSilence( RMSWindow *rmsw, short *block, int nsamples, long *p_rms )
{
	long	rms;
	long	window_avg, window_stddev;

	rms = calculate_rms( block, nsamples );
	rms *= 100;
	rms /= MAX_HWORD;
	*(rmsw->window_ptr++) = rms;
	if ( rmsw->window_ptr == rmsw->window_end )
	{
		rmsw->window_ptr = rmsw->window_start;
		window_stddev = calculate_stddev( rmsw->silence_window, WINDOW_SIZE, &window_avg );
		if ( (window_avg - window_stddev < rmsw->silence_threshold) &&
		     (window_stddev < 10) )
	{
			rmsw->silence_threshold += window_avg + window_stddev;
			rmsw->silence_threshold /= 2;
			rmsw->thresholdx2 = rmsw->silence_threshold * 2;
		}
	}

	if ( rms > rmsw->thresholdx2 )
		rmsw->silence_count = 0;		// talk spurt detected.
	else if ( rms > rmsw->silence_threshold )
	{
		if ( rmsw->silence_count > 0 ) rmsw->silence_count--;
	}
	else
		rmsw->silence_count++;
	TRACE("rms = %.2f%%, silence threshold = %.2f%%, silence count = %d\n",
		(double)rms/100, (double)rmsw->silence_threshold/100, rmsw->silence_count);
	if ( p_rms != NULL ) *p_rms = rms;
	return rmsw->silence_count >= SILENCE_LIMIT;
}

#define MAX_DEVICETHRESHOLD		8
static  int			nb_devThreshold = 0;
static  DeviceThreshold		*devThreshold[ MAX_DEVICETHRESHOLD ];

DeviceThreshold *SND_getDeviceThreshold( const char *device_path, int device_index, int rw_mode, int op_mode )
{
	int	i;
	DeviceThreshold		*dt = NULL;

	for( i=0; i<nb_devThreshold; i++)
	{
		if ( !strcmp( device_path, devThreshold[i]->device_path ) &&
		      device_index == devThreshold[i]->device_index )
		{
			dt = devThreshold[i];
			break;
		}
	}
	if ( dt == NULL && nb_devThreshold < MAX_DEVICETHRESHOLD )
	{
		dt = malloc( sizeof(DeviceThreshold) );
		memset( dt, 0, sizeof(DeviceThreshold) );
		strncpy( dt->device_path, device_path, 32 );
		dt->device_path[31] = '\0';
		dt->device_index = device_index;
		devThreshold[nb_devThreshold++] = dt;
		RMSW_init( &dt->rms_read, 1000 );	// read start from 10% -- drop down on stable silence RMS very soon
		RMSW_init( &dt->rms_write, 200 );	// write is start from 1% -- release sound out quickly.
	}
	if ( dt != NULL )
	{
		if ( rw_mode == O_RDONLY )
			dt->in_mode = op_mode;
		else
			dt->out_mode = op_mode;
		dt->refer++;
	}
	else
	{
#if 0
		TRACE("Failed to obtain DeviceThreshold for %s (index: %d) with %s %s\n",
			device_path, device_index,
			rw_mode==O_RDONLY ? "READ" : "WRITE",
			op_mode==SNDMODE_HALF_DUPLEX ? "HalfDuplex" : "FullDuplex" );
#endif
	}
	return dt;
}

void SND_destroyDeviceThreshold( DeviceThreshold *dt )
{
	int	i;

	for( i=0; i<nb_devThreshold; i++)
		if ( devThreshold[i] == dt ) 	break;
	if ( i < nb_devThreshold )
	{
		dt->refer--;
		if ( dt->refer == 0 )
		{
			free( dt );
			for ( ; i<nb_devThreshold-1; i++)
				devThreshold[i] = devThreshold[i+1];
			nb_devThreshold--;
		}
	}
}
